#include "Path.h"
#include "MyGL.h"
#include "Pathfinding.h"
#include <iostream>

namespace Engine
{
	Path::Path()
	{
	}

	void Path::InitPath(const Vec3& start, const Vec3& end)
	{
		m_start	= start;
		m_end	= end;
	}

	void Path::DrawPath()
	{
		if(m_path.empty())
			return;

		glColor3f	(1.0f, 1.0f, 1.0f);

		// ---> debug purposes only
		//
		glBegin(GL_LINES);

		for(int i = 0; i < m_path.size() - 1; i++)
		{
			glVertex3f	(m_path[i].x, 0.0f, m_path[i].y);
			glVertex3f	(m_path[i + 1].x, 0.0f, m_path[i + 1].y);
		}
		glEnd();

		glBegin(GL_LINES);
			glVertex3f	(5.0f, 0.0f, -5.0f);
			glVertex3f	(m_t.x, m_t.y, m_t.z);
		glEnd();
	}

	Vec3 Path::MapPointToPath(const Vec3& point)
	{
		// ---> point to line distance
		// ---> should return a point from the path
		//

		Vec3 result;
		float minDist = FLT_MAX;
		float d;

		for(int i = 0; i < m_path.size() - 1; i++)
		{
			Vec3 start = Vec3(m_path[i].x, 0.0f, m_path[i].y);
			Vec3 end = Vec3(m_path[i + 1].x, 0.0f, m_path[i + 1].y);
			
			d = PointToSegmentDistance(point, start, end);

			if(d < minDist)
			{
				minDist = d;
				result = m_chosen;
			}
		}

		return result;
	}

	float Path::PointToSegmentDistance(const Vec3& point, Vec3& p0, Vec3& p1)
	{
		Vec3 m = p1 - p0;
		Vec3 q = point - p0;

		m.Normalize();

		float t = DotProduct(m, q) / DotProduct(m, m);

		// ---> check for endpoints
		if(t < 0)
		{
			//m_chosen = (q);

			//return m_chosen.Length();

			m_chosen = p0;
			t = 0;
			return Vec3::Distance(point, p0);
		}

		if(t > m.Length())
		{
			//m_chosen = (point - p1);
			//return m_chosen.Length();
			m_chosen = p1;
			t = m.Length();
			return Vec3::Distance(point, p1);
		}
		//else
		//{
			//Vec3 p = p0 + (m * t);

			//m_chosen = point - p;

			//return m_chosen.Length();
			m_chosen = p0 + (m * t);
			return Vec3::Distance(point, m_chosen);
		//}
	}

	void Path::SetT(Vec3 t)
	{
		m_t = t;
	}

	void Path::PopulateVec(Vec2 n)
	{
		//if(!m_path.empty())
		//	m_path.erase(m_path.begin(), m_path.end());

		Vec2 result = ThePathfinding::Instance()->ToWorldCoords(n);
		m_path.push_back(result);
	}
}